package com.hitqz.robot.driver;

import cn.hutool.core.bean.BeanUtil;
import cn.hutool.core.util.ObjectUtil;
import cn.hutool.core.util.StrUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.alibaba.fastjson.JSONObject;
import com.google.common.cache.Cache;
import com.google.common.collect.Lists;
import com.google.common.collect.Maps;
import com.hitqz.robot.api.business.enums.CmdNavStatusEnum;
import com.hitqz.robot.api.business.enums.RobotDriverEnum;
import com.hitqz.robot.api.business.enums.RobotOtherCommandEnum;
import com.hitqz.robot.api.common.dto.form.FormConfigDto;
import com.hitqz.robot.api.common.dto.robot.*;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMap;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.api.common.util.CacheNameUtil;
import com.hitqz.robot.common.core.util.R;
import com.hitqz.robot.driver.dto.map.MapDataDto;
import com.hitqz.robot.driver.dto.robot.RobotPushDto;
import com.hitqz.robot.driver.dto.robot.RobotQueryMapDto;
import com.hitqz.robot.driver.dto.robot.RobotQueryPathDto;
import com.hitqz.robot.driver.dto.robot.RobotQueryStationsRetDto;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.model.MapAttributesDto;
import com.hitqz.robot.driver.model.PointDto;
import com.hitqz.robot.driver.model.RobotIOStatusDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.hitqz.robot.driver.service.NettyChannelService;
import com.hitqz.robot.driver.util.RobotStatusUtil;
import com.hitqz.robot.driver.util.SeqUtil;
import com.hitqz.robot.driver.util.SourceEnum;
import lombok.extern.slf4j.Slf4j;
import org.springframework.retry.annotation.Backoff;
import org.springframework.retry.annotation.Retryable;
import org.springframework.stereotype.Service;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.nio.charset.StandardCharsets;
import java.util.*;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;

/**
 * @author xupkun
 * @date 2023/8/2
 */
@Slf4j
@Service("robotDriverService")
public class XgRobotDriverImpl implements RobotDriverService {

    private static RobotMap map;


    @Override
    public boolean doOnlineMode(Robot robot) {
        NettyChannelService.connect(robot.getIpAddress(), null);
        //切在线状态
        RobotOnlineEventDto dto = new RobotOnlineEventDto();
        dto.setOnlineStatus(1);
        SpringUtil.getApplicationContext().publishEvent(dto);
        return true;
    }

    @Override
    public boolean motionAbort() {
        return navCmd(CmdNavStatusEnum.NAV_CANCEL.getCode());
    }

    @Override
    public boolean navCmd(Integer cmd) {
        robotConfigLockReq();
        log.info("导航控制:" + cmd);
        if (CmdNavStatusEnum.NAV_PAUSE.getCode() == cmd.intValue()) {
            customSendMessage(SourceEnum.ROBOT_NAV.getSource(), 0x0BB9, null);
        } else if (CmdNavStatusEnum.NAV_CANCEL.getCode() == cmd) {
            customSendMessage(SourceEnum.ROBOT_NAV.getSource(), 0x0BBB, null);
        } else if (CmdNavStatusEnum.NAV_CONTINUE.getCode() == cmd) {
            customSendMessage(SourceEnum.ROBOT_NAV.getSource(), 0x0BBA, null);
        }
        return true;
    }

    @Override
    public R executeOtherCommand(RobotOtherCommandEnum cmdEnum, String jsonParams) {
        int cmd = cmdEnum.getXgCommand();
        int result = -1;
        if (cmd == 0x17B6) {
            // 顶升起来
            // 6070
            robotConfigLockReq();
            if (StrUtil.isNotEmpty(jsonParams)) {
                // "shelf/s0002.shelf"
                log.info("带货架的顶升上升命令");
                Map<String, Object> params = Maps.newHashMap();
                params.put("source_id", "SELF_POSITION");
                params.put("id", "SELF_POSITION");
                params.put("operation", "JackLoad");
                params.put("recfile", jsonParams);
                NettyChannelService.sendMessage(
                        SourceEnum.ROBOT_NAV.getSource(), SeqUtil.nextId(), 3051,
                        JSONObject.toJSONString(params).getBytes(StandardCharsets.UTF_8)
                );
            } else {
                log.info("普通顶升上升命令");
                customSendMessage(SourceEnum.ROBOT_OTHER.getSource(), cmd, null);
            }
        } else if (cmd == 0x17B7) {
            // 6071
            // 顶升下降
            log.info("顶升下降命令");
            robotConfigLockReq();
            customSendMessage(SourceEnum.ROBOT_OTHER.getSource(), cmd, null);
        } else if (cmd == 9300) {
            // 设置超市时间
            Map<String, Object> map = Maps.newHashMap();
            map.put("interval", 2000);
            customSendMessage(SourceEnum.ROBOT_MESSAGE_PUSH.getSource(), 9300, map);
        } else if (cmd == 1027) {
            Cache<String, RobotPushDto> cache = SpringUtil.getBean(CacheNameUtil.CACHE_ROBOT_INFO);
            if (cache.asMap().size() > 0) {
                for (String key : cache.asMap().keySet()) {
                    RobotPushDto pushDto = cache.getIfPresent(key);
                    result = pushDto.getJackStatusDto().getJackState();
                }
            }
            return R.ok(result);
        } else if (cmd == RobotOtherCommandEnum.RE_LOCATION.getXgCommand()) {
            log.info("下发重新定位");
            robotConfigLockReq();
            Map<String, Object> ret = Maps.newHashMap();
            ret.put("isAuto", true);
            customSendMessage(SourceEnum.ROBOT_CONTROLLER.getSource(), 0x07D2, ret);
        } else if (cmd == RobotOtherCommandEnum.ROD_CONTROLLER.getXgCommand()) {
            robotConfigLockReq();
            customSendMessage(SourceEnum.ROBOT_CONFIG.getSource(), 0x0FA5, null);
        } else if (cmd == RobotOtherCommandEnum.CANCEL_CONTROLLER.getXgCommand()) {
            log.info("释放");
            robotConfigLockReq();
            customSendMessage(SourceEnum.ROBOT_CONFIG.getSource(), 0x0FA6, null);
        } else if (cmd == RobotOtherCommandEnum.CLEAR_ERROR_MESSAGE.getXgCommand()) {
            log.info("清除错误");
            robotConfigLockReq();
            customSendMessage(SourceEnum.ROBOT_CONFIG.getSource(), 0x0FA9, null);
        }
        return R.ok();
    }

    @Override
    public IRobotPosDto getWebPos() {
        // 导航状态: 0-none 1- 等待 2-执行中 3-暂停 4-完成 5- 失败 6-取消
        Cache<String, RobotPushDto> cache = SpringUtil.getBean(CacheNameUtil.CACHE_ROBOT_INFO);
        IRobotPosDto dto = new IRobotPosDto();
        if (cache.asMap().size() > 0) {
            dto = new IRobotPosDto();
            dto.setUrl(Optional.ofNullable(map).map(RobotMap::getFilePath).orElse(""));
            dto.setCurrentMap(Optional.ofNullable(map).map(RobotMap::getMapCode).orElse(""));
            for (String key : cache.asMap().keySet()) {
                RobotPushDto pushDto = cache.getIfPresent(key);
                dto.setReached((pushDto.getTaskStatus() == 4));
                BeanUtil.copyProperties(pushDto, dto);
                dto.setBatteryLevel(BigDecimal.valueOf(dto.getBatteryLevel() * 100).setScale(2, RoundingMode.HALF_UP).floatValue());
                double speed = 0.0;
                if (cache.getIfPresent(key) != null) {
                    speed = cache.getIfPresent(key).getVx() > 0 ?
                            cache.getIfPresent(key).getVx() : cache.getIfPresent(key).getVy();
                }
                List<RobotIOStatusDto> diStatus = Lists.newArrayList();
                List<RobotIOStatusDto> doStatus = Lists.newArrayList();
                pushDto.getDI().forEach(t -> {
                    RobotIOStatusDto robotIOStatusDto = new RobotIOStatusDto();
                    robotIOStatusDto.setId(t.getId());
                    robotIOStatusDto.setStatus(t.isStatus());
                    diStatus.add(robotIOStatusDto);
                });
                pushDto.getDO().forEach(t -> {
                    RobotIOStatusDto robotIOStatusDto = new RobotIOStatusDto();
                    robotIOStatusDto.setId(t.getId());
                    robotIOStatusDto.setStatus(t.isStatus());
                    doStatus.add(robotIOStatusDto);
                });
                dto.setDI(diStatus);
                dto.setDO(doStatus);
                dto.setTargetStation(RobotStatusUtil.TARGET_POS_NAME);
                dto.setNavStatus(pushDto.getTaskStatus());
                dto.setSpeed(new BigDecimal(speed).setScale(2, RoundingMode.HALF_UP).doubleValue());
                dto.setTaskStatus(pushDto.getTaskStatus());
                dto.setCurrentDirection(pushDto.isCharging() ? "充电" : "放电");
                dto.setLastStation(pushDto.getTaskStatusPackage().getClosestTarget());
                dto.setJackHeight(pushDto.getJackStatusDto().getJackHeight());

                //
                dto.setFull(pushDto.getJackStatusDto().isJackIsFull());
                /**
                 * 坐标换算
                 */
                if (map != null && map.getHeight() != null && map.getWidth() != null) {
                    PointDto pointDto = point2Canvas(map, new PointDto(dto.getX(), dto.getY()));
                    dto.setPixelX(Double.valueOf(pointDto.getX()).intValue());
                    dto.setPixelY(Double.valueOf(pointDto.getY()).intValue());
                    //
                    dto.setPixelAngle((float) (dto.getAngle() * (Math.PI / 180)));
                }
                break;
            }
        }
        return dto;
    }

    private PointDto point2Canvas(RobotMap robotMap, PointDto pointDto) {
        PointDto pixelPoint = new PointDto();
        pixelPoint.setX((pointDto.getX() - robotMap.getMinX()) * robotMap.getWidth() / (robotMap.getMaxX() - robotMap.getMinX()));
        pixelPoint.setY((robotMap.getMaxY() - pointDto.getY()) * robotMap.getHeight() / (robotMap.getMaxY() - robotMap.getMinY()));
        return pixelPoint;
    }

    @Override
    @Retryable(maxAttempts = 10, backoff = @Backoff(delay = 1000L, multiplier = 1))
    public List<RobotMapPos> getNavPath(RobotMapPos sourcePos, RobotMapPos targetPos, List<RobotMapPos> allRobotMapPos) {
        Lock lock = RobotStatusUtil.ROBOT_QUERY_PATH;
        if (lock == null) {
            lock = new ReentrantLock();
            RobotStatusUtil.ROBOT_QUERY_PATH = lock;
        }
        synchronized (lock) {
            try {
                Map<String, Object> params = Maps.newHashMap();
                params.put("source_id", sourcePos.getCode());
                params.put("target_id", targetPos.getCode());
                customSendMessage(SourceEnum.ROBOT_STATUS.getSource(), 0x0517, params);
                lock.wait(5000);
            } catch (Exception e) {
                log.info("解锁异常");
            }
            RobotQueryPathDto queryPathDto = RobotStatusUtil.ROBOT_QUERY_PATH_DATA;
            log.info("{}--{} 获取到的json{}", sourcePos.getCode(), targetPos.getCode(), JSONObject.toJSONString(queryPathDto));
            List<RobotMapPos> robotMapPosList = new ArrayList<>();
            if (queryPathDto == null) {
                throw new IllegalStateException("获取数据失败，需要重试");
            }
            try {
                queryPathDto.getList().forEach(t -> {
                    allRobotMapPos.stream().filter(allRobot -> allRobot.getCode().equals(t))
                            .findFirst().ifPresent(robotMapPosList::add);
                });
                RobotStatusUtil.ROBOT_QUERY_PATH_DATA = null;
            } catch (Exception e) {

            }
            return robotMapPosList;
        }
    }

    @Override
    @Retryable(maxAttempts = 5, backoff = @Backoff(delay = 1000L, multiplier = 1))
    public List<RobotMapPos> getRobotMapPos(String mapCode) {
        Lock lock = RobotStatusUtil.ROBOT_QUERY_MAP_POS;
        if (lock == null) {
            lock = new ReentrantLock();
        }
        try {
            synchronized (lock) {
                try {
                    customSendMessage(SourceEnum.ROBOT_STATUS.getSource(), 0x0515, null);
                    lock.wait(5000);
                } catch (Exception e) {
                }
            }
            RobotQueryStationsRetDto dto = RobotStatusUtil.ROBOT_STATIONS;
            List<RobotMapPos> robotMapPosList = new ArrayList<>();
            if (dto != null) {
                dto.getStations().forEach(t -> {
                    RobotMapPos robotMapPos = new RobotMapPos();
                    robotMapPos.setCode(t.getId());
                    robotMapPos.setName(t.getId());
                    robotMapPos.setPosX(Double.parseDouble(t.getX()));
                    robotMapPos.setPosY(Double.parseDouble(t.getY()));
                    robotMapPos.setYaw(0);
                    robotMapPos.setPosZ(0.0);
                    robotMapPosList.add(robotMapPos);
                });
            } else {
                throw new IllegalArgumentException("Invalid");
            }
            return robotMapPosList;
        } catch (Exception e) {

        }

        return null;
    }

    @Override
    @Retryable(maxAttempts = 5, backoff = @Backoff(delay = 1000L, multiplier = 1))
    public List<RobotMap> getMapList() {
        Lock lock = RobotStatusUtil.ROBOT_QUERY_MAP;
        if (lock == null) {
            lock = new ReentrantLock();
        }
        try {
            synchronized (lock) {
                try {
                    log.info("get map list");
                    customSendMessage(SourceEnum.ROBOT_STATUS.getSource(), 0x0514, null);
                    lock.wait(3000);

                } catch (Exception e) {

                }
            }
        } catch (Exception e) {

        }
        RobotQueryMapDto dto = RobotStatusUtil.ROBOT_QUERY_MAP_DATA;
        if (dto == null) {
            log.info("retrying");
            throw new IllegalStateException("retrying");
        }
        List<RobotMap> robotMaps = Lists.newArrayList();
        dto.getMaps().forEach(t -> {
            RobotMap robotMap = new RobotMap();
            robotMap.setMapCode(t);
            robotMap.setMapName(t);
            robotMap.setMapOriginx(0);
            robotMap.setMapOriginy(0);
            robotMap.setMapResolution(0);
            robotMaps.add(robotMap);
        });
        return robotMaps;
    }

    @Override
    public String getCurrentMap() {
        String mapName = "";
        Cache<String, RobotPushDto> cache = SpringUtil.getBean(CacheNameUtil.CACHE_ROBOT_INFO);
        IRobotPosDto dto = new IRobotPosDto();
        if (cache.asMap().size() > 0) {
            for (String key : cache.asMap().keySet()) {
                mapName = Objects.requireNonNull(cache.getIfPresent(key)).getCurrentMap();
                break;
            }
        }
        return mapName;
    }

    @Override
    public MapAttributesDto getMapAttributes(String mapCode) {
        MapAttributesDto mapAttributesDto = new MapAttributesDto();
        Lock lock = RobotStatusUtil.ROBOT_QUERY_MAP_ATTRIBUTES;
        if (lock == null) {
            lock = new ReentrantLock();
            RobotStatusUtil.ROBOT_QUERY_MAP_ATTRIBUTES = lock;
        }
        try {
            synchronized (lock) {
                Map<String, Object> params = Maps.newHashMap();
                params.put("map_name", mapCode);
                customSendMessage(SourceEnum.ROBOT_CONFIG.getSource(), 0x0FAB, params);
                lock.wait(2000);
                MapDataDto mapDataDto = RobotStatusUtil.ROBOT_QUERY_ATTRIBUTES_DATA;
                if (mapDataDto != null) {
                    mapAttributesDto.setCurrentMapName(mapCode);
                    mapAttributesDto.setMapOriginx(mapDataDto.getHeader().getMinPos().getX());
                    mapAttributesDto.setMapOriginy(mapDataDto.getHeader().getMinPos().getY());
                    mapAttributesDto.setMapResolution(mapDataDto.getHeader().getResolution());
                }
            }
        } catch (Exception e) {
        }
        // 获取地图的数据
        return mapAttributesDto;
    }

    @Override
    public void selMap(RobotMap map) {
        XgRobotDriverImpl.map = map;
        log.info("选择地图:{}", JSONObject.toJSONString(map));
    }

    @Override
    public PointDto canvas2Point(RobotMap robotMap, PointDto pointDto) {
        // 8489 * 3764 2D的计算方式
        PointDto calPos = new PointDto();
        double xResolution = robotMap.getMapResolution();
        double yResolution = robotMap.getMapResolutionY();
        BigDecimal middleX = new BigDecimal(pointDto.getX() * xResolution).setScale(4, RoundingMode.HALF_UP);
        BigDecimal middleY = new BigDecimal(-pointDto.getY() * yResolution).setScale(4, RoundingMode.HALF_UP);
        log.info("middleX {} ---- middleY {}", middleX.doubleValue(), middleY.doubleValue());
        log.info("x---{} --- y {}-", robotMap.getMapOriginx(), robotMap.getMapOriginy());
        calPos.setX(middleX.doubleValue() + robotMap.getMapOriginx());
        calPos.setY(middleY.doubleValue() + robotMap.getMapOriginy());
        //x/8489*82.692
        return calPos;
    }

    @Override
    public FormConfigDto getRobotConfig(String formName) {
        return RobotDriverService.super.getRobotConfig(formName);
    }

    @Override
    public RobotDriverEnum getDriverVersion() {
        return RobotDriverEnum.XG;
    }

    @Override
    @Retryable(maxAttempts = 10, backoff = @Backoff(delay = 1000L, multiplier = 1))
    public boolean robotTaskGoTarget(RobotGoTargetPosDto dto) {
        robotConfigLockReq();
        log.info("前往目标点: {}", dto.getCode());
        Lock lock = RobotStatusUtil.ROBOT_GO_TARGET_POS;
        if (lock == null) {
            lock = new ReentrantLock();
            RobotStatusUtil.ROBOT_GO_TARGET_POS = lock;
        }
        synchronized (lock) {
            try {
                Integer cmd = 3051;
                Map<String, Object> params = Maps.newHashMap();
                params.put("source_id", "SELF_POSITION");
                params.put("id", dto.getCode());
                NettyChannelService.sendMessage(
                        SourceEnum.ROBOT_NAV.getSource(), SeqUtil.nextId(), cmd,
                        JSONObject.toJSONString(params).getBytes(StandardCharsets.UTF_8)
                );
                RobotStatusUtil.TARGET_POS_NAME = dto.getCode();
                lock.wait(1000);
            } catch (Exception e) {
                log.info("解锁异常");
            }
            Boolean result = RobotStatusUtil.ROBOT_GO_TARGET_RESULT;
            log.info("前往站点结果:" + result);
            if (result == null || !result) {
                throw new IllegalStateException("前往目标点失败，请清除任务重新开始");
            }
            //if (dto.getRobot().getDispatch() == 1) {
            //    Cache<String, Integer> cmdNav = SpringUtil.getBean("cmdNav");
            //    //Integer cmd = cmdNav.getIfPresent(robot.getRobotSn());
            //    cmdNav.put(dto.getRobot().getRobotSn(), CmdNavStatusEnum.NAV_PAUSE.getCode());
            //    log.info("支持是否接受调度系统: 暂停导航等待调度命令" );
            //    this.navCmd(1);
            //}
        }


        return true;
    }


    @Override
    @Retryable(maxAttempts = 10, backoff = @Backoff(delay = 1000L, multiplier = 1))
    public boolean robotTaskGoTargetList(RobotGoTargetPosListDto dto) {
        robotConfigLockReq();
        log.info("指定路径前往目标点: {}", dto.getMoveTaskList());
        Lock lock = RobotStatusUtil.ROBOT_GO_TARGET_POS_LIST;
        if (lock == null) {
            lock = new ReentrantLock();
            RobotStatusUtil.ROBOT_GO_TARGET_POS_LIST = lock;
        }
        synchronized (lock) {
            try {
                Integer cmd = 3066;
                NettyChannelService.sendMessage(
                        SourceEnum.ROBOT_NAV.getSource(), SeqUtil.nextId(), cmd,
                        JSONObject.toJSONString(dto).getBytes(StandardCharsets.UTF_8)
                );
                lock.wait(1000);
            } catch (Exception e) {
                log.info("解锁异常");
            }
            Boolean result = RobotStatusUtil.ROBOT_GO_TARGET_RESULT;
            log.info("指定路径前往站点结果:" + result);
            if (result == null || !result) {
                throw new IllegalStateException("指定路径前往目标点失败，请清除任务重新开始");
            }
        }
        return true;
    }


    @Override
    public boolean setInitPos(InitPosDto baseInitPosDto) {
        return false;
    }

    @Override
    public boolean setSpeed(RobotMoveDto robotMoveDto) {
        byte[] data = new byte[10];
        robotConfigLockReq();
        Integer cmd;
        Map<String, Object> params = Maps.newHashMap();
//        直行优先
        log.info("发送速度值:" + JSONObject.toJSONString(robotMoveDto));
        if (!("0").equals(robotMoveDto.getDist())) {
            cmd = 0x0BEF;
            params.put("dist", BigDecimal.valueOf(Double.parseDouble(Optional.ofNullable(robotMoveDto.getDist()).orElse("0"))).setScale(2, RoundingMode.HALF_DOWN).floatValue());
            params.put("vx", BigDecimal.valueOf(Double.parseDouble(robotMoveDto.getLinerSpeed())).setScale(2, RoundingMode.HALF_DOWN).floatValue());
            params.put("vy", BigDecimal.valueOf(Double.parseDouble(robotMoveDto.getLinerSpeedY())).setScale(2, RoundingMode.HALF_DOWN).floatValue());
            customSendMessage(SourceEnum.ROBOT_NAV.getSource(), cmd, params);
            return true;
        } else if (!("0").equals(robotMoveDto.getAngle())) {
            cmd = 0x0BF0;
            params.put("angle", BigDecimal.valueOf(Double.parseDouble(robotMoveDto.getAngle())).setScale(2, RoundingMode.HALF_DOWN).floatValue());
            params.put("vw", BigDecimal.valueOf(Double.parseDouble(robotMoveDto.getAngularSpeed())).setScale(2, RoundingMode.HALF_DOWN).floatValue());
            data = JSONObject.toJSONString(params).getBytes(StandardCharsets.UTF_8);
            NettyChannelService.sendMessage(SourceEnum.ROBOT_NAV.getSource(),
                    SeqUtil.nextId(), cmd, data);
            return true;
        }
        return false;
    }

    @Override
    public boolean changeWebControlSpeed(float changeValue) {
        return false;
    }


    public void robotConfigLockReq() {
        Integer cmd = 0x0FA5;
        Map<String, Object> params = Maps.newHashMap();
        params.put("nick_name", "hit");
        customSendMessage(SourceEnum.ROBOT_CONFIG.getSource(), cmd, params);
    }

    public void customSendMessage(String sourceEnum, Integer cmd, Map params) {
        byte[] data = null;
        if (ObjectUtil.isNotEmpty(params)) {
            data = new byte[10];
            data = JSONObject.toJSONString(params).getBytes(StandardCharsets.UTF_8);
        }
        NettyChannelService.sendMessage(sourceEnum,
                SeqUtil.nextId(), cmd, data);
    }


    public static void main(String[] args) {

        RobotGoTargetPosListDto dto=new RobotGoTargetPosListDto();
        List<RobotMoveTargetDto> list=new ArrayList<>();
        RobotMoveTargetDto d1=new RobotMoveTargetDto();
        d1.setId("123");
        d1.setSourceId("xxx");
        d1.setTaskId("zzz");
        RobotMoveTargetDto d2=new RobotMoveTargetDto();
        d2.setId("333");
        d2.setSourceId("www");
        d2.setTaskId("ddd");
        list.add(d1);
        list.add(d2);
        dto.setMoveTaskList(list);
        log.info(JSONObject.toJSONString(dto));
    }


}
